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(54) THREE-DIMENSIONAL POSITION POSTURE SENSING DEVICE 

(57)Abstract: 

PROBLEM TO BE SOLVED: To obtain a three-dimensional position posture sensing 
device capable of stably estimating the three-dimensional position posture of an 
object without being affected by shield and the like. 

SOLUTION: A three-dimensional position posture sensing device has an image input 
means for inputting an image 5 taken by an image device and of which three 
dimensional position information for a measuring object 1 is known and at least three 
markers 2 are imaged, a region extraction means extracting the region corresponding 
to each marker 2 on the image 5, a marker identifying means identifying individual 
means from the characteristic of outline of the marker 2 in the extracted region, and 
a position posture operation means operating the three dimensional position posture 
of the measuring object for the imaging device by using the position on the image 5 of 
each identified marker 2 and the three dimensional position posture of the measuring 
object of each marker 2. 
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(3) , cHm <4) «9 4iSO«?^m-f5^i:*S-e# 
5. 

[0 118] £T, K^^tVfca- K-r-*cr>5 fcfiHW 

Ms. .... Matt*. 
[0 1 1 9] &&&gc®ftmmaKm (k) (fc40 

= 1. 2, 3, 4) fc#LT\ mt LtSt>^**?UV^ 
^^5>M 4 , Ms, ... , M m &m% L-"Cife}£ 

[0 12 0] (1) #«eHm (k) K*H-3S?IHE3Sc 

dist (k) srft/Nc-ra k fiirw^ryT-c^c 

[0121] (2) #«cHb (k) (k=l, 2, 
3, 4) (CiatT^T^fel-«kt), dist (k) * 

[0122] a) dist (k) : = 0 fc^NffiBi**:^ 50 



[0 12 3] b) H!^Stvfc^««30 3^(CjStR$^^ 
T'.pofc-^— #M j ( j = 4 , 5 m) l£oV> 

o, y>*> *j») Sr, cHm (k) kmm\sX** 

[0 12 4] (uj ' , vj ') ir-f 

[Ski 2] 

yj m 

Zj m 
1 



U'j" 






0 


uo 


0 " 




Vj 




0 


Qfy 


VO 


0 


cH m (k) 


W'i 




0 


0 


0 


0 





V j 

u> }~ yyT V 'J- VV^j 
[0 12 6] g£^T, ^-#MjC03Sg8(Cp«&f*|-?jHilJ£ 

**v-t22te7£(arg ( UJ , vj) (u 

j ', vj *) i:02SiaS3Se j SrSCttJ-t-*. 
[0 12 6] r CD 2 ?gaS2£ e j I*, &T<0 £ ? tC LT» 

[0 12 7] 

ej = (uj '— uj )* + (vj '— vj) 2 

UT, dist (k) I*, 
I&13] 

dist(k) = £ e ; = £ {(u'j - u^Hv'i - Vj ) 2 ] 

[0 128] (3) &.±.0>£5\Zl,X&.$>t->frlZd i s 
t (k) iJS/hir^S^^^J^cHm (k) Sr 



7rfj9 ^ifll 
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15 

[0 12 9] &m-?2>k> ±&<DXv L yy-e#it>ti>frZ> 
SiSlcHm (k) ti, =— Kv-AMi, M 2 , M 3 

Ms M m t>s&%3t&i-2>M*mtRTz 

to iso] (5) m<owm 

±&*°rvrf (4) ffS^JcHm (k) ti, = 
— K^-#Mi, Mb, M 3 a>'=>*£^i*it5t:k£>T , fc 
D , tt«-7-*M4. Mo, ... , Mm^M-f^S'J^ 

[0 13 1] (5) -hSB-^y 1 



16 

•>-^ (4) -?#ttj£:h.fc)$?cHro (k) 4rD3Sl!l{6^<Ec 
Hm(«) tit, f^a-pv-^MI ( i = 1 , 

2 m) {££0. r.<Og?<DS3^^?f 5, 

[0 13 2] -r/,£fc>fe, cHmSr^g^ (roll 
( <f> *) — pi tch (*y) — yaw (<* x ) ) bW. 
it$# (t x , ty, t») }iSB9t-C< 6 ?fc7C?fc*fl^ 
j^P = ( * x, * y, * x i t x, t y. t t) 'i L, 

.*©*!»HS»tt«r p <°> = ( * x <« . * y W , * z 



(0) 



cHm E 



-6in«y cos*ysfnO x 
0 0 



. t*co> . t y (o> , t z <°> ) tSBtS, 
[0133] flflsttldtt, 

0 1 



0 0 0 1 



•lO-l-3-4l^iHSr^^ hJtt«3RT^^-# 3 #20 
TcffiS (xiS yi «, zi") is ^C0^7^5Si^ 



1, 2, . . . , m) CHUT, 
[gfel 6] 



(i = 



7' 



t.) «:Hgf-r*^i:Sr^^<5o 



[0 13 5] 



(xt ■ 


, y i ■ 


, zjO <!:> 




V 


i) OHflitt 


[&1 5] 








"Ui" 




a u 0 uo 




Vi 




0 Ofg Y0 




Wi 

* ■ 




0 0 0 



Wj 









o" 


cHm 


yi m 


0 




0 




1 _ 


Vl 




Wj 





fj(p;xi m ,yi m .zi m ;ij|.vi,) 



_rfi 1 (p;xi m ,yi m ,zi m ;ui,vi)'] m 



t,») fc/BV'T, 6&.5b-><7 

4> y> <t> z i t xi tyi t i) t 



t x«» . t r W 

J* — & p = ( <#> X, 

[0137] rOTfflj@«i<*ofcnfc^»^as;P4JS 

[0 13 8] :o£^ 1:1X6^7^-^11 -r^< 

50 



[0 13 9] *fr*&l &BttijnB$aKS3 £ro 

[0 14 0] «Jioj;5*»lO3*0filO»«lcJ;*ttf, 

3 i:o|Wo3J>S7£tt*HflHr*ia-r*ii:aJ-C#*. 

[0141] sfc, -?-*o«atc»uri*, v-^B 
*««*l**»=Mi'<-c»jftnrti±**«ri:tf!-ce»o 
[0142] (02 (Dnmnmrn) mz. m 2 con&o 
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[0143] ±&Litmi<nmMv>Mi&x-te, m®&& 

iS3 t L,X#9-ffi&*£.!&X-SZt>toZfemL., * 
[0 14 41 CHtttU ** 2 ©JSJfi0>#1»-Ctt» # 

[0 14 5] 2 <0HJfi<D^T?f4, »£d£*l10 

[oi4 6] ±aufc»i«>*i6©»jio±5te*9^ 

#MMBa**#V»b - 20. 

[0 14 7] *3$ffi<7>M1&X-3tWtZ>ttX'te. Z<0£ 

[0148] *j(Jk^ftoS^fefl|Mf*R9 l -cat* 
*tlfc*tl*fcRI'l*T*&» tojlllfl^fy^it, B8-C 

JBttft 3 9 iZ* / 9 f'MIra > f 
^-^ 4("^ffl1-S. 

[0 1.4 9] y/2m #7— S?{£o5«;;b30 

5, 

[0 15 0] ±fc, ^yt/SKSSI^Ttt:, ^— #igi£ 

[0 15 1] &lblc» *?y7'4fc:*JV v CW:, ^ — #SB 

So 40 
[0 15 2] EtT\ *OS¥SB&, **ry-7l, 3, 

[0 15 3] MMOJNI-Vti, s-KtT 

[0 15 4] *TV-f2 ! @9«, JH2©«||0»fflK: 
*5lt5^v l 5'> r 2T-O*aa03^)lfi«'^'t _ 7o— ^■•r— h 

[0 15 5] ^S'T'l 1TN4, lii*Jftfl$tft{S3ic£9 



10 A'-jT 

18 

is *. n z y 4 xj&# tp**-*- z . 

,[015 7] i<D®«^9l7'A'5fy X^, £ LTH ±5* 
(K. Rahardjaand A. Kos 
aka "Vision— based bin — pick 
ing:Recognition and local 
ization of multiple compl 
ex objects using simplevi 
sual cues, "Proceedings of 
1 9 9 6 1 E E E/R SJ Internatlo 
nal Conference on Intel 1 i 
gent Robots and Systems, O 
saka, Japan. November 1996) 
iZTp&toX^^^S pedge-an d— Me d g e 
--^-SnBU-Cr. Pavlidis and Y. Li 
ow, "Integrating region gr 
owing and edge detect io 
n, "IEEE Transactions on P 
attern' Analysis and Mac hi 
ne Intelligence, Vol. 12, N 
o. 3, pp. 2 2 5 — 2 3 3, 1 9 9 0) lzm£to& 
Spl i t — and — M e r g e ^SHK Cannyx 

rv&taxmxtem ufc* * &&&&&& \~xmm&w 

[0 15 8] <fclc, ?f y^l 3 y [z Lt 

[0 15 9] *0®«k|cMtr r®1SA 

<k) j , r^:t$L (k) j . mm<z>¥-®m.m 

(k) J , rfiAoaHBffi» i (k) J ^ifSrgffl-T 
[0 16 0] iftfeowlSfitf. ^— j&.t LT5lf3 o 5 
[0 16 1] Afrttfcl*. iS&S-IWMfcPs fcfefettft 
[0 16 2] Jb9B*4M*fta*, BSJA (k) % 

(k) . siowftn <k) , wi&<oi&mu 

Mb (k) o4&-g-(^r±. 

[#17 J 

Amin<A(k)<:Amax 
Lmin < L(k) < Lmax 
mmin < m(k) < mmax 
snriin < s(k) < smax 

tri"-<T*fc urt>5 1 1rtot£*. v^o 
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[0 16 3] ZZX% Am i n, Am a x if l^Ili. 

I0 16 4J Z<D&*>l£l.X&%tl1SiVftmi&(Z>tpfrlb-* 

[0 1 6 5 J figWC, IS? 1 tfCiSiR-C" 

[0 16 6] :<5^, jS*W»£ 3 -K-^-Jtr 

[0 16 7] *£T?. 0ift38ft'?4ittt2:*ttfc«Hft 

[0168] £eO^JS»-«lCoV>T«, 

[0 16 9] rn-?^Ty7'2li»Tt5 0 
[0 17 0] ^fy7"3 ; T-toX. 1 ? V zf 3 K&^T, & 
1 ©**©#flto»*£J>«*a*#?H*0Wtt*<^ 20 

[0 17 1] ^fj'7'4;v?f7/3t ( a— >t— * 



20 

— Kt— *OT3^7Ct£a <xi», yj», Zj"), 
*^7Ufe®IC*J»tS1ttR (us, vi) , a— K-^— 

[0 17 2] =— K-v— #©WWMC 

#t*a«Rifi««>*tt©ft* r i Srfijffl LT, jtr* 50 

[0 17 3] <1) Pffli»j^3Sa3*»6,->?-*2*T?ro 

[0174] sxTz<o&mz!SLw-rz> n 

[0 1 7 5 J mi O^jpi-J; 5 iz s -v—;&cDtp'D>£ 
Pi. 5<a£.£«r0e t-T5i:i:*>^ % Steffi 

[0 17 6] v-AOSScJcff^^SRi TO 

[0177] 
[St 18] 



UQ 



— _ VI- VQ 
V| = — 1 — " 



Ri 



cos 0 i 



[0178] * UT, z i £ d i oWCtt, »T©H« 



: 7T = zi : 1 
[«ki 9] 
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21 22 

zj = d|ce»3©i 



dii± <d * ^ \=.&m T? # £ . 



4- * 



rjcos 2 © i 



. or uvRi 
5d t = --p4s-<?r, 



[0 17 9] <2) #&#5l iSifgljffi^gS3f$^3& [0 18 0] i i^-* j^tLT. ^W3»; 

mmmwrn - - *o • 7SttEm*R t *M?»j*o e (fc&«tB) t 

i«) ^»W-r*^teitoV^-CS^SiL, «Sv>f^ [&2 0] 



~~ * U| - uo uj - UO Vi - vo V] - vo 

cos 0 « u U 



+1 



[0 1 8 1] «S-r-*©SflBJB«*t»OSJfc3BttB»flt [0 18 4] J; 5 C d i 0&JSfi££3J3H- 

(i) »*- h>-$:&mmi-?>jjm 

[0 1.8 2] HA, Z.<nmM* * aklrtltt, H3S*«0 (2) h^SrfiJffl-r-O^jfe 

[&2 1] ^J>#<<0^*s^^fc^« (1 

fij = di 2 + dj 2 - 2d|djcos Sg-Rij^O [0185] &mifco>J&1&'eU: < 3 ) n*/^ ^7 

*Sj££L^tHl2fc&>fcV\, [0 18 6] iSB-CSiigSiXTVSJgfflld i ( i = 1, 

[0 18 3]tSoT, CT^^JfT-C-^fe^^fcd i<D^ 2, .... n) Ztegnb-t h/Vp 4r*H U * 

SfiSi d iCOSaS^St/f&^JCc os (9 i j) coSSg w$##tWry'J2:S, -Tfc;fc><b 

^«c*?tJffi-r*ii:twJ:!), d i©*J«HIMr*BN-*J: [f2 2] 



7?tv9 fiSIf 11 
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23 - 24 

P l {di l <jb l ... l dn] T , S^diag (<sr 12,0-2^..., cr n 2) 

3fn _ 'afij J afij _ 

£*8i#'£#;l<&p CdJC 2d|-2djcasey |4= 2dj - 2dicos e ij 

•easy, ' 



d cos ^ i] 5 cos £ II 

2d!dj auj ; >2d,dj av> -2d,dj- 



d cos 0 j 
8 Uj 



3 cos £ ifl 

.2didj-^j 



= =l=hl =V(ut)2+ (v7)2 hj =V(PJ) 2+ ffi> 2 sii'tfjtJj+'viyf oos9ij«-j^: 

Bihihj-^ ^hihj.^i' 



0 cos 0 i 



«v 



h;2hj2 " T hj2hj2 " h[2hj2 

. . ,-P.ii{dl.d2,...,drt]3, _ S =dj.ag [?ri2,cr& v „crtfi) ._. 
A =dlag (cru2.O-v2.0ru2.o-v2) 

for (i=0; i<n; i++) { 
for (1=1+1; j<n;J++) { 



h|2hj2 



— ui-uc 
9fii 



9P 



8 cos 6 ]j 



gjfu _r, 

8qij [' 
K=SM T (U + MSM 1 ")" 1 



3 cos 5 ij 9 cos S n d cos 



Stij 



9 



ss£jf| 
VJ J 



Pnew 13 p - Kfjj 
Snaw - ( I • KM ) S 

p = S = Snew 1= «fc U MfcM ^M&f 

} 

} 



[0 13 7] CCOdH;*^ 7^^<73JE^^e>-^-*^ 
[0 18 8] c®B»WM?T?*ai:, SSlo^SSojl* 

a$T-# a. 



[0190] (35 3 onjgo^fig) mz, t$3<nmi&<7> 

[oi9i] ±^iRxfmz<Dmm(Dmm^^^x 
14, mm&frb-r— fifaWkiB.&tLZteiSi&ibm'fzv 



7rfv9 & iff? 92 
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[0 19 2] ua>$f£, *«WT»Ht, »*«Ue-*— *fc 

[o 1 9 3 j -?rc-e> *%$&<fi>wm~n\L vm®.*^-* 

<V%M<D&m\c&^T, -r-*«*£*ttta-r5i: it io 

[0 19 4] SiP^<OB5^-h, *»*»»ttl-C«, ff*2<D 

[0 19 5] b38vL*36«'fev-J(ri-O*»«)*tt-C»tW-b-20- 
[0196] **tte&tt-ci±. ®&<0;*:# * &JKPi<fe 

1/1 6-{s«/jNUfc*^te-»tNT. stm-s.. 

[0 19 7] S^WKtts E6<0^^y7'HC33(^S^ 

[0198] y r 1 : JRVNRtr 1/16 fc*f /J> 

■fS (f?2r|S]l£l/4, yiJ^lRjIC 1/4 . 

[0 19 9] liKiIi&c9lil&&?ij£ ( i , j ) b 
U »Mftol{(i!n<r (is. j s) b-tSb. i 
- 4 * i s + 0 , i=4*I 5 + 1, i=4*i s+ 30 
2, i =4 * i s +3©* IlcatLT, *>-t> j =4 * j 
8+0, j=4* j s + l, j=4*js + 2, j=4 
* j b + 3lc*t^-« J K*3lt£> 1 0PHfO¥ftMKfe ( i 

5 a 

[0 2 0 0] t^fyT'S : 1 

[0 2 0 1 ] iWttaj^Stt, Mil W^JfecO^fiiSfeia 
&2<D&M<omi&X'M<<btl1tjjmb\simX'hZ> <> 40 
[0 2 0 2] ^^f-fZfS : ^y^9 fc 5'^2TfaaS 

Hfc0«<ott®asfii (i 5 , j s) Z4&-r^>'tzzb 

[0 2 03] tU-to i 3 3 *>££a»tt-?gl93 Lfc 

^m%bzzb\z.£y), w.m&<n*% mat^x 1 / 1 

[0 2 04] (ft4*>S£*>7*tt) ®4O^Jfic0 

7*fflfco^T»w-*-a. so 



14 V 

26 

[0 2 0 5] iOS4C9S5Jferojgtg-C«:, =r — Kt— # 

cta-o^vfw— teasiiS'tft^ofifs 

[0 2 0 6] Ctl.ST'{C^iaUfcmi75MS3©||J£<0 
Affile J:S*«tet?r±s => — K-*— #<D 
3 ^TcteS^T'^ t * ft>Vfe^O*tfl*a0> 2 &5EttE 4 

[0 2 0 7] roj:5ft*ait4, => — 

[0 2 0 8] L^LfctfSfc. i£*S&tf>te@gS3Hi;t;SS3S 

[0209] sjLTi=.s&^ ; i>m4<omn<ni&mz£Z¥m 
jwm b mm is ttftwrnmrnntotoM *mm 

— 

[0 2 10] £/T, Sfe^^OHSfSor iSr^V 

[0211] (HJ60J 1 ) *1* , Xlfcffll 1 4 Lt, =» — 

tefa«rf£?t-r£ 1 4 fen. *©«)fe*nfc«fflfta«>w 
a. 

[0 2 12] VS, 7t7i?*L? hffilSI3R-c, a— K^— 

*0, 1 m-USfiS^h^po, 

p i, Pn-iT?SS^, 9>K^— ^ kofa: 
gx^hyupk^s, po, pi. . . . Pin-lt»f 
*5£-?3r£*i,5i:-f Sir (bo. bi b 

[«t23] 

Pk- 0OPO+ ^1P1+ • • • + £m-iPm-1 

^C0M^f^T?O{S:Eqk = (ur, v k ) tt, a— 
-*OW«rtSHB«qo = (U 0 , vo ") , q,= 
(«1 . vi) , q m - t = (un-i , v„-i ) b 

r 

[«C2 4] 

qk ^->S oqo + fi iqi + - • ■ + fi m-iqm-1 

X i ?m-t&Z.btfiX'$Z> e 

[0213] i (D^mm.&mm t, qk <Di&&X&t>& 
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[0 2 14) El lte, BJ48f*?*»fe^^ K-r-^^ttffl 
[0 2 151 wW0fJ-Ctt. 3-l<7-*0, 1. 2riSfc 

[02181 t*i, ®^rt-etttB*^5t=i- K-e— * 

0, i, 2©w«rtttR©s:«fr«rJR»>. •eosm^f+ia-r- 

[0 2 17] co?ip(cLT9>K^-^k^stJ^-r5 

— * k»*£Si: Lra@§Jx5. 
[0 2 181 coiiicut, =» — K-^— Jt&by^ K 

[0 2 l-9-]-*fcv-»l«*«i:.UXlH:.-».2-<0**O»20.. 
SrgHLfctfX^ ^1 ^ISO^fieo^KOlSJlff^ffiT? 
*t*» l 3SIH-3 s £ a«-c* a. 

[0 2 2 01 0MKM2) ii-Ca«^5ISSfiW2<0#}!fe 

[0 2 2 1] -tux, 9 v k-t— * *s|sj£-c»* fcw-g-u 

[0 2 2 2] J^T, *©3trftlcoy^-CJKW-f*,- 
[0 2 2 3J V*, B*rt<>6a-Kv-#0, 

1 m- ltfspQJt StvfciffiJg-rst, 

o^-^i^ h^^T«<D3^7cffi:SJSfi?&-iii^-C-fi-40 
«*itfc2*5cffia*r«lfflbr. M*4b l «> s )k5StiEA 

[0 2 2 4] £©J&r8M*» ®lfcSV^4Sf2 0^KUO?g 

■•wifcwiiiiWitJb*. 

[0 2 2 S]VVfc. C0 3ft7BttSS^O«»g»tSffi«r 

[0 2 2 6] ±fc, 5>K^k<0t^i^hig 
SKfcltSteSS: (xk. yk. 2k) iTSi. 7V 
K^"-* k©B®ftl£*Stt5^g!lfiE Uk, vk) l*5£ 

(3) , (4) fi£DlSttl"f*ri:^t?#*o 50 



15 a"-** 

28 

[0 2 2 7] wOt-S^S (uk. vk) ©/^Sat 5 , BH& 
[0 2 2 8] 5kL-C\ K-?— ?&tbm • 

T, «t««&l«0 3^>S«r{aSgS&0}tE^4rK«f-rSii: 
[0 2 2 9] ^_h, =» 

[0 2 3 0] Z<O^miz£<0. $><ofr£&&mi, 

[0 2 3 1] (»5 0||JfeO®ffl() i<0^5(D^J&fir?)g 

[0 2 3 2] Z.tnkXiz&.WVX%K.$8l 75M4CDHJS 
©Jfcfigra. ^T^t>. ittSii &mtSMeg*B3lH)0!>tt: 

[o-2 3-3i-L*>*fc. i vmmttte&Kom&b isX, 

[0 2 3 4] Z(Om^-±, .MtWMWSia^V'" 

[02ssigii2(s s ^5 <z>mi&<oi&m'?mfe£tiz> 

[0 2 3 61 Ttefr-b. mi 2 (-jjrf £ r> h*-^ 
— * 1 2 2*S^§nt^H) 12 1 Sr*S»1-5WflK« 

2 4tig|aL > =>-lfa— ^ 1 2 4r±-t©iU 
ffc^— * SrflWf 3 r. i ic * 9 .. *J-&<& 12 1 
S^y-^* hffig?^*»fc®^»^«l 2 3»s«^-f 

[0 2 8 7]'-*. 

[0 2 3 8] -ttOM^^v^— ^^Jfflbr, =^ 

[0 2 3 9] J&MHRaifcttS-i-SSAi 2 5 

*tU-C, Bff«MR»*iSi 2 3*«^iry^^— s^b^-p* 

[0240] j:o AttM^i*. i{$i^sgi 2 3 am 
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[0 2 4 1] 3Vtf» — 9 1 2 4d«i®&a»e ) ^' — 

*ixl* c HtnT-g-f-r t is-J?t So 
[0 2 4 2] wSn,&2«co?F2fe^mff?iJ*»6.. 
* hE«^a»fcSi*JBSfc3&'>*©#*gmfTyiJfr, SIT© 

[0 2 4 8] 

[&2 5] 10 
Mm - rHg cHm 

[0 2 4 4] *£6O*ft*>9tt£J:0fc*S£9£, 

[0 2 4 S] (Iff 6 <03?Jfe©7i5JS) ^<0^6(7?^J6C0^ 

[ 0-2 4 6 ] 3B*r-8 KJ3 3 Jte7E^Sr^20 

[0 2 4 7] ^©ffeiLTIl F lash Poin 

[0248] Flash Point <0 J: 5 tiftX?''* 

[0 2 4 9] — jir. ^W^ICIS, 7^ -V 30 

i / -f 5 * v * 5 mJSWKift> & «. 

[0 2 5 0] ;*£tt4>»ttr*. ittfeoBSS^«r<W*i- 
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